#include "Quaternion.h"
#define _USE_MATH_DEFINES
#include <math.h>

Quaternion::Quaternion(float x, float y, float z, float w)
:	x(x), y(y), z(z), w(w) {
}

Quaternion::Quaternion(const Vector3D& v, float angle) {
	float sinAngle;
	// convert from degrees to radians
	angle *= (float) (M_PI / 180);
	angle *= 0.5f;
	Vector3D vn(v.x, v.y, v.z);
	vn.normalize();

	sinAngle = sinf(angle);
					   
	x = (vn.x * sinAngle);
	y = (vn.y * sinAngle);
	z = (vn.z * sinAngle);
	w = cosf(angle);
}
					   
Quaternion::Quaternion() {
	Vector3D vector;
	vector.x = 0; vector.y = 1; vector.z = 0;
	setByAxisAngle(vector, 0);
}
	
Quaternion Quaternion::getConjugate() const {
	return Quaternion(-x, -y, -z, w);
}
	
// normalising a quaternion works similar to a vector. This method will not do anything
// if the quaternion is close enough to being unit-length. define TOLERANCE as something
// small like 0.00001f to get accurate results
void Quaternion::normalize() {
	const float tolerance = 0.00001f;
	// Don't normalize if we don't have to
	float mag2 = x * x + y * y + z * z + w * w;
	if (  mag2!=0.f && (fabs(mag2 - 1.0f) > tolerance)) 
	{
		float mag = sqrt(mag2);
		x /= mag;
		y /= mag;
		z /= mag;
		w /= mag;
	}
}
	
// Convert from Axis Angle
void Quaternion::setByAxisAngle(const Vector3D& v, float angle) {
	float sinAngle;
	// convert from degrees to radians
	angle *= (float) (M_PI / 180);
	angle *= 0.5f;
	Vector3D vn(v.x, v.y, v.z);
	vn.normalize();
		
	sinAngle = sinf(angle);
		
	x = (vn.x * sinAngle);
	y = (vn.y * sinAngle);
	z = (vn.z * sinAngle);
	w = cosf(angle);
}
	
// Convert to Matrix
Matrix Quaternion::getMatrix() const {
	float x2 = x * x;
	float y2 = y * y;
	float z2 = z * z;
	float xy = x * y;
	float xz = x * z;
	float yz = y * z;
	float wx = w * x;
	float wy = w * y;
	float wz = w * z;
		
	// This calculation would be a lot more complicated for non-unit length quaternions
	Matrix res;

	res._11 = 1.0f - 2.0f * (y2 + z2);
	res._12 = 2.0f * (xy - wz);
	res._13 = 2.0f * (xz + wy);
	res._14 = 0.0f;
		
	res._21 = 2.0f * (xy + wz);
	res._22 = 1.0f - 2.0f * (x2 + z2);
	res._23 = 2.0f * (yz - wx);
	res._24 = 0.0f;
		
	res._31 = 2.0f * (xz - wy);
	res._32 = 2.0f * (yz + wx);
	res._33 = 1.0f - 2.0f * (x2 + y2);
	res._34 = 0.0f;
		
	res._41 = 0.0f;
	res._42 = 0.0f;
	res._43 = 0.0f;
	res._44 = 1.0f;

	return res;
}

float Quaternion::dot( const Quaternion& a ) const {
	return x * a.x + y * a.y + z * a.z + w * a.w;
}
	
void Quaternion::nlerp(const Quaternion& a, const Quaternion& b, float t) {
	Quaternion q = a*(1-t) + b*t;
	q.normalize();
	*this = q;
}

void Quaternion::slerp(const Quaternion& a, const Quaternion& b, float t) {
	float w1, w2;
		
	float cosTheta = a.dot(b);
	float theta    = (float)acos(cosTheta);
	float sinTheta = (float)sin(theta);
		
	if( sinTheta > 0.001f )
	{
		w1 = float( sin( (1.0f-t)*theta ) / sinTheta);
		w2 = float( sin( t*theta) / sinTheta);
	} else {
		// CQuat a ~= CQuat b
		w1 = 1.0f - t;
		w2 = t;
	}
		
	*this = a*w1 + b*w2;
}
	

// Multiplying a quaternion q with a vector v applies the q-rotation to v
Vector3D Quaternion::rotateVector(const Vector3D &vec) const {
	Vector3D vn(vec);
	vn.normalize();
		
	Quaternion vecQuat(vn.x, vn.y, vn.z, 0.0f);
	Quaternion resQuat;
		
	resQuat = vecQuat * getConjugate();
	resQuat = *this * resQuat;
		
	return (Vector3D(resQuat.x, resQuat.y, resQuat.z));
}
	
Vector3D Quaternion::getDirectionVector() {
	// positive Z-axis set as default movement direction (upwards on display in initial position)
	Vector3D v(0, 0, 1);
	v = this->rotateVector(v);
	return v;
}

// concatenating two quaternions, returns (*this) * rq
Quaternion Quaternion::operator* (const Quaternion& rq) const {
	return Quaternion(w * rq.x + x * rq.w + y * rq.z - z * rq.y,
						w * rq.y + y * rq.w + z * rq.x - x * rq.z,
						w * rq.z + z * rq.w + x * rq.y - y * rq.x,
						w * rq.w - x * rq.x - y * rq.y - z * rq.z);
}

Quaternion Quaternion::operator*(float scalar) const {
	return Quaternion(x * scalar, y * scalar, z * scalar, w * scalar);
}
	
Quaternion Quaternion::operator+ (const Quaternion& quaternion) const {
	return Quaternion(x + quaternion.x, y + quaternion.y, z + quaternion.z, w + quaternion.w);
}

Quaternion Quaternion::getRotationAroundYAxis(float angle) {
	const Vector3D yAxis(0, 1, 0);
	Quaternion rotationAroundYAxis;
	rotationAroundYAxis.setByAxisAngle(yAxis, angle);
	return rotationAroundYAxis;
}